#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <iostream>
#include <vector>

class TeleopKeyboardNode : public rclcpp::Node
{
public:
  TeleopKeyboardNode() : Node("teleop_keyboard")
  {
    publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
      "/forward_position_controller/commands", 10);
    
    // 关节初始值，可按需修改
    joint_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0};
  }

  // 非阻塞模式读取键盘输入
  char get_key()
  {
    struct termios oldt, newt;
    char ch = 0;
    
    // 获取当前终端设置
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    
    // 设置为无缓冲模式
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    
    // 设置非阻塞模式
    int oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
    
    // 读取输入
    read(STDIN_FILENO, &ch, 1);
    
    // 恢复终端设置
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    fcntl(STDIN_FILENO, F_SETFL, oldf);
    
    return ch;
  }

  void run_teleop()
  {
    RCLCPP_INFO(this->get_logger(), "使用 W/S 控制第一个关节，Q 退出。");
    RCLCPP_INFO(this->get_logger(), "当前关节位置：[%.2f, %.2f, %.2f, %.2f, %.2f]", 
                joint_positions_[0], joint_positions_[1], joint_positions_[2], 
                joint_positions_[3], joint_positions_[4]);
                
    rclcpp::WallRate loop_rate(10);  // 10Hz
    
    while (rclcpp::ok())
    {
      char key = get_key();
      bool changed = false;
      
      if (key == 'w' || key == 'W')
      {
        joint_positions_[0] += 0.1;
        changed = true;
        RCLCPP_INFO(this->get_logger(), "增加第一个关节值：%.2f", joint_positions_[0]);
      }
      else if (key == 's' || key == 'S')
      {
        joint_positions_[0] -= 0.1;
        changed = true;
        RCLCPP_INFO(this->get_logger(), "减少第一个关节值：%.2f", joint_positions_[0]);
      }
      else if (key == 'q' || key == 'Q')
      {
        RCLCPP_INFO(this->get_logger(), "退出键盘控制");
        break;
      }
      
      if (changed)
      {
        auto message = std_msgs::msg::Float64MultiArray();
        message.data = joint_positions_;
        publisher_->publish(message);
      }
      
      rclcpp::spin_some(this->get_node_base_interface());
      loop_rate.sleep();
    }
  }

private:
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_;
  std::vector<double> joint_positions_;
};

int main(int argc, char* argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TeleopKeyboardNode>();
  node->run_teleop();
  rclcpp::shutdown();
  return 0;
}
